'''
Created on Feb 18, 2019

@author: mody
'''

import math 
import numpy as np
import matplotlib.pyplot as plt


class Bicycle(object):
    """Implementation of the famous bibycle "model"
    Inicialize the model, getting **wheelBase** = distance between the wheels and
    **phi_max** = maximun steer angle
    **The distances must be ALLWAYS in meters**"""

    def __init__(self, L ,phi_max ,steps,delta_t):
        # wheelBase is the length between the wheels
        self.wheelBase = L
        # phi_max is the maximal angle of the                     
        self.phi_max = phi_max
        # steps          
        self.steps = steps
        # delta time             
        self.delta_t = delta_t
        #Array of x-position for generating vehicle path
        self.x = np.zeros(self.steps,dtype = np.double)    
        #Array of y-position for generating vehicle path                          
        self.y = np.zeros(self.steps,dtype = np.double)
        #Array of theta for generating vehicle path                             
        self.theta = np.zeros(self.steps,dtype = np.double) 
        # (0,0,0) and the pose is (x,y,theta)                     
        self.xPose =0.0                             
        self.yPose = 0.0                     
        self.thetaPose = 0.0 
        
        self.thetaDeg = np.zeros(self.steps,dtype = np.double) 
                             
    def clear(self):
        """Clear the list of poses"""
        # (0,0,0) and the pose is (x,y,theta)    
        self.xPose =0.0                             
        self.yPose = 0.0                    
        self.thetaPose = 0.0     
        
    def getPose(self):
        """Return que actual pose of the model. The pose is in (x,y,theta) form"""
        return self.xPose, self.yPose, self.thetaPose
    
    def setPose(self,x,y,theta):
        """Define a new pose to the model"""
        #clear all the arrays
        self.x = np.zeros(self.steps,dtype = np.double)                        
        self.y = np.zeros(self.steps,dtype = np.double)                        
        self.theta = np.zeros(self.steps,dtype = np.double)                    
   
        # (0,0,0) and the pose is (x,y,theta)    
        self.xPose = x                             
        self.yPose = y                     
        self.thetaPose = theta 
        self.x[0] = self.xPose
        self.y[0] = self.yPose
        self.theta[0] = self.thetaPose
        

    def update(self,dx,dy,dtheta,step_number):
        """Change the pose of the model adding the parameters to the actual pose"""
        self.xPose +=(dx*self.delta_t)
        self.yPose +=(dy*self.delta_t)
        self.thetaPose += (dtheta*self.delta_t)

        self.x[step_number] = self.xPose
        self.y[step_number] = self.yPose   
        self.theta[step_number] = self.thetaPose
        self.thetaDeg[step_number] = math.degrees(self.thetaPose)

    def run(self,__v,__phi,step_number):
        """Calculate the next pose of the model getting the speed (distance travaled
        in this step time) and the steer angle"""
        #limits the steer angle to the fisical limit
        if(__phi > self.phi_max):   
            __phi = self.phi_max
        if(__phi < -self.phi_max):
            __phi = -self.phi_max
            
        delta_x = __v*math.cos(self.thetaPose)
        delta_y = __v*math.sin(self.thetaPose)
        delta_theta = (__v/self.wheelBase)*math.tan(__phi)
        
        self.update(delta_x,delta_y,delta_theta,step_number)
        
    def show(self,counter):
        plt.figure(1)
        
        x_plot = plt.subplot(2, 2, 1)
        x_plot.set_title('X-Position')
        x_plot.plot(counter,self.x,'ro')
        
        
        y_plot = plt.subplot(2, 2, 2)
        y_plot.set_title('Y-Position')
        y_plot.plot(counter,self.y,'ro')
        
        p_plot = plt.subplot(2, 2, 3)
        p_plot.set_title('Path')
        p_plot.plot(self.x,self.y,'bo')
        
        theta_plot = plt.subplot(2, 2, 4)
        theta_plot.set_title('Orientation')
        theta_plot.plot(counter,self.thetaDeg,'yo')
        
        plt.show()